#! /home/a/anaconda3/envs/ros/bin/python
from time import sleep
import numpy as np
import rospy
from unitree_legged_msgs.msg import LowCmd, MotorCmd

def servo():
    rospy.init_node('unitree_gazebo_test_py')
    robot_name = rospy.get_param('/robot_name')
    print('robot name:', robot_name)

    lowCmd = LowCmd()
    servo_pub = list(np.arange(12))
    servo_pub[0] = rospy.Publisher("/" + robot_name + "_gazebo/FR_hip_controller/command", MotorCmd, queue_size=1)
    servo_pub[1] = rospy.Publisher("/" + robot_name + "_gazebo/FR_thigh_controller/command", MotorCmd, queue_size=1)
    servo_pub[2] = rospy.Publisher("/" + robot_name + "_gazebo/FR_calf_controller/command", MotorCmd, queue_size=1)
    servo_pub[3] = rospy.Publisher("/" + robot_name + "_gazebo/FL_hip_controller/command", MotorCmd, queue_size=1)
    servo_pub[4] = rospy.Publisher("/" + robot_name + "_gazebo/FL_thigh_controller/command", MotorCmd, queue_size=1)
    servo_pub[5] = rospy.Publisher("/" + robot_name + "_gazebo/FL_calf_controller/command", MotorCmd, queue_size=1)
    servo_pub[6] = rospy.Publisher("/" + robot_name + "_gazebo/RR_hip_controller/command", MotorCmd, queue_size=1)
    servo_pub[7] = rospy.Publisher("/" + robot_name + "_gazebo/RR_thigh_controller/command", MotorCmd, queue_size=1)
    servo_pub[8] = rospy.Publisher("/" + robot_name + "_gazebo/RR_calf_controller/command", MotorCmd, queue_size=1)
    servo_pub[9] = rospy.Publisher("/" + robot_name + "_gazebo/RL_hip_controller/command", MotorCmd, queue_size=1)
    servo_pub[10] = rospy.Publisher("/" + robot_name + "_gazebo/RL_thigh_controller/command", MotorCmd, queue_size=1)
    servo_pub[11] = rospy.Publisher("/" + robot_name + "_gazebo/RL_calf_controller/command", MotorCmd, queue_size=1)

    for i in range(4):
        lowCmd.motorCmd[i*3+0].mode = 0x0A
        lowCmd.motorCmd[i*3+0].Kp = 70     # desired position stiffness位置刚度 (unit: N.m/rad )
        lowCmd.motorCmd[i*3+0].dq = 0      # desired velocity (unit: radian/second)
        lowCmd.motorCmd[i*3+0].Kd = 3      # desired velocity stiffness速度刚度 (unit: N.m/(rad/s) )
        lowCmd.motorCmd[i*3+0].tau = 0     # desired output torque扭矩 (unit: N.m)
        lowCmd.motorCmd[i*3+1].mode = 0x0A
        lowCmd.motorCmd[i*3+1].Kp = 180
        lowCmd.motorCmd[i*3+1].dq = 0
        lowCmd.motorCmd[i*3+1].Kd = 8
        lowCmd.motorCmd[i*3+1].tau = 0
        lowCmd.motorCmd[i*3+2].mode = 0x0A
        lowCmd.motorCmd[i*3+2].Kp = 300
        lowCmd.motorCmd[i*3+2].dq = 0
        lowCmd.motorCmd[i*3+2].Kd = 15
        lowCmd.motorCmd[i*3+2].tau = 0

    pos = [0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 
            0.0, 0.67, -1.3, -0.0, 0.67, -1.3]
    for j in range(12):
        lowCmd.motorCmd[j].q = pos[j]
    for i in range(500):
        for m in range(12):
            servo_pub[m].publish(lowCmd.motorCmd[m])
        sleep(0.001)
    print('init finish.')

    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():

        # robot_controller.sendServoCmd()
        rate.sleep()


if __name__ == '__main__':
    try:
        servo()
    except rospy.ROSInterruptException:
        pass